/* 编码器库 - 双编码器示例
 * http://www.pjrc.com/teensy/td_libs_Encoder.html
 *
 * 此示例代码属于公共领域
 */

#include <Encoder.h>

// 修改这些引脚号为连接到您编码器的引脚
//   最佳性能：两个引脚都具有中断功能
//   良好性能：仅第一个引脚具有中断功能
//   低性能：两个引脚都没有中断功能
Encoder encoderLeft(5, 6);    // 左轮编码器，连接到引脚5和6
Encoder encoderRight(7, 8);   // 右轮编码器，连接到引脚7和8
//   避免使用带有LED的引脚

void setup() {
  Serial.begin(115200);       // 初始化串口通信，波特率115200
  Serial.println("DiffBot Wheel Encoders:"); // 打印初始化信息
}

// 用于存储编码器位置的变量
long positionLeft  = -999;    // 左轮位置
long positionRight = -999;    // 右轮位置

void loop() {
  long newLeft, newRight;
  newLeft = encoderLeft.read();   // 读取左轮编码器值
  newRight = encoderRight.read(); // 读取右轮编码器值
  
  // 当任一编码器的值发生变化时
  if (newLeft != positionLeft || newRight != positionRight) {
    Serial.print("Left = ");
    Serial.print(newLeft);        // 打印左轮编码器值
    Serial.print(", Right = ");
    Serial.print(newRight);       // 打印右轮编码器值
    Serial.println();
    positionLeft = newLeft;       // 更新左轮位置
    positionRight = newRight;     // 更新右轮位置
  }
  
  // 新增功能：当编码器值达到特定阈值时，打印一条消息
  if (newLeft > 1000 || newRight > 1000) {
    Serial.println("Encoder value exceeded threshold!");
  }
  
  // 如果从串口监视器发送任何字符
  // 将两个编码器值都重置为零
  if (Serial.available()) {
    Serial.read();
    Serial.println("Reset both wheel encoders to zero"); // 打印重置信息
    encoderLeft.write(0);         // 重置左轮编码器
    encoderRight.write(0);        // 重置右轮编码器
  }
}
